/*****************************************************************

Kelly Ward
COMP 236
Homework #1, Part b
File: robot1_wardk.cpp

*****************************************************************/

#include "parallelepiped.h"
#include "sphere.h"
#include "pyramid.h"
#include "tetrahedron.h"
#include "cylinder.h"
#include "cone.h"
#include "drawer.h"
#include "collision.h"
#include "utils.h"

#include <stdlib.h>
#include <GL/glut.h>
#include <math.h>
#include <iostream>
#include <cstdio>
using namespace std;

Parallelepiped * par, *par2;
Sphere * esfera;
Pyramid * pyr, * pyr2;
Tetrahedron * tet, * tet2;
Cylinder * cyl, * cyl2;
Cone * cone;
Drawer * drawer;
Collision *collision;
Point3D * p1, * p2, *p3;
Segment3D seg;

//Camera controls
GLdouble eyex, eyey, eyez, atx, aty, atz, upx, upy, upz;
bool locked;

// ROBOT ARM CONTROLS
	// Initial Positions
double BaseTransX=-0.5f;  // 0
double BaseTransZ=0;      
double BaseSpin=0;        // 1
double ShoulderAng=-10;   // 2
double ElbowAng=-90;
double WristAng=90;       // 3
double WristTwistAng=10;
double FingerAng1=45;     // 4
double FingerAng2=-90;

// ROBOT COLORS
GLubyte Arms[] = { 128,128,128 };
GLubyte Joints[] = { 0,68,119 };
GLubyte Fingers[] = { 150,0,150 };
GLubyte FingerJoints[] = { 128,128,128 };
GLubyte teapotColor[] = { 128,128,0 };


// USER INTERFACE GLOBALS
int LeftButtonDown=0;    // MOUSE STUFF
int OldX,OldY,NewX,NewY;
int RobotControl=1;

// LIGHTS
bool lights[] = { true, true, true, true, true, true };

// Constants
double rotateAngle = M_PI/180.;

// Define light positions (need to be called after viewport)
void lightPositions() {
	GLfloat light_position0[] = { 1.0, 0.0, 1.0, 0.0 };
	GLfloat light_position1[] = { 1.0, 0.0, -1.0, 0.0 };
	GLfloat light_position2[] = { -1.0, 0.0, 1.0, 0.0 };
	GLfloat light_position3[] = { -1.0, 0.0, -1.0, 0.0 };
	GLfloat light_position4[] = { 0.0, 2.0, 0.0, 0.0 };
	GLfloat light_position5[] = { 0.0, 1.0, 0.0, 0.0 };
	glLightfv(GL_LIGHT0, GL_POSITION, light_position0);
	glLightfv(GL_LIGHT1, GL_POSITION, light_position1);
	glLightfv(GL_LIGHT2, GL_POSITION, light_position2);
	glLightfv(GL_LIGHT3, GL_POSITION, light_position3);
	glLightfv(GL_LIGHT4, GL_POSITION, light_position4);
	glLightfv(GL_LIGHT5, GL_POSITION, light_position5);
}

void cameraMove(double ex, double ey, double ez, double ax, double ay, double az) {
	int aux;
	locked = false;
	aux = CMP(ex, eyex);
	if(aux != 0) {
		locked = true;
		eyex += double(aux)*0.01;
	}

	aux = CMP(ey, eyey);
	if(aux != 0) {
		locked = true;
		eyey += double(aux)*0.01;
	}

	aux = CMP(ez, eyez);
	if(aux != 0) {
		locked = true;
		eyez += double(aux)*0.01;
	}

	aux = CMP(ax, atx);
	if(aux != 0) {
		locked = true;
		atx += double(aux)*0.01;
	}

	aux = CMP(ay, aty);
	if(aux != 0) {
		locked = true;
		aty += double(aux)*0.01;
	}

	aux = CMP(az, atz);
	if(aux != 0) {
		locked = true;
		atz += double(aux)*0.01;
	}
}

void DrawUnitCylinder(int NumSegs)  // x,y,z in [0,1], Y-axis is up
{
  int i;
  double *Px = new double[NumSegs];	//Pontos da circunferencia
  double *Py = new double[NumSegs];
  double AngIncr = (2.0f*3.1415927f) / (double)NumSegs;
  double Ang = AngIncr;
  Px[0]=1;
  Py[0]=0;
  for (i=1; i<NumSegs; i++, Ang+=AngIncr)
  {
    Px[i]=(double)cos(Ang);
    Py[i]=(double)sin(Ang);
  }

  glMatrixMode(GL_MODELVIEW);
  glPushMatrix();
    glTranslatef(0.5f,0.5f,0.5f);
    glScalef(0.5f,0.5f,0.5f);

    // TOP
    glNormal3f(0,1,0);
    glBegin(GL_TRIANGLE_FAN);
    glVertex3f(0,1,0);
    for (i=0; i<NumSegs; i++)
      glVertex3f(Px[i],1,-Py[i]);
    glVertex3f(Px[0],1,-Py[0]);
    glEnd();

    // BOTTOM
    glNormal3f(0,-1,0);
    glBegin(GL_TRIANGLE_FAN);
    glVertex3f(0,-1,0);
    for (i=0; i<NumSegs; i++)
      glVertex3f(Px[i],-1,Py[i]);
    glVertex3f(Px[0],-1,Py[0]);
    glEnd();

    // SIDES
    glBegin(GL_QUAD_STRIP);//GL_QUAD_STRIP
    for (i=0; i<NumSegs; i++)
    {
      glNormal3f(Px[i],0,-Py[i]);
      glVertex3f(Px[i],1,-Py[i]);
      glVertex3f(Px[i],-1,-Py[i]);
    }
    glNormal3f(Px[0],0,-Py[0]);
    glVertex3f(Px[0],1,-Py[0]);
    glVertex3f(Px[0],-1,-Py[0]);
    glEnd();

  glPopMatrix();

  delete[] Px;
  delete[] Py;
}

void DrawUnitSphere(int NumSegs)  // x,y,z in [0,1]
{
  glMatrixMode(GL_MODELVIEW);
  glPushMatrix();
    glTranslatef(0.5f,0.5f,0.5f);
    glutSolidSphere(0.5f,NumSegs,NumSegs);
  glPopMatrix();
}

void DrawUnitCone(int NumSegs)  // x,y,z in [0,1], apex is in +Y direction
{
  glMatrixMode(GL_MODELVIEW);
  glPushMatrix();
    glTranslatef(0.5f,0,0.5f);
    glRotatef(-90, 1,0,0);
    glutSolidCone(0.5f,1,NumSegs,NumSegs);
  glPopMatrix();
}

void DrawGroundPlane(int NumSegs)
{
  glColor3f(0.7f,0.7f,0.7f);
  /*glBegin(GL_QUADS);
    glNormal3f(0,1,0);
    glVertex3f(-1,0,1);
    glVertex3f(1,0,1);
    glVertex3f(1,0,-1);
    glVertex3f(-1,0,-1);
  glEnd();
  */
  glBegin(GL_LINE_LOOP);
    glVertex3f(-1,0,1);
    glVertex3f(1,0,1);
    glVertex3f(1,0,-1);
    glVertex3f(-1,0,-1);
  glEnd();
}

void DrawJoint(int NumSegs)
{
  glMatrixMode(GL_MODELVIEW);
  glPushMatrix();
    glScalef(0.15f,0.15f,0.12f);
    glRotatef(90,1,0,0);
    glTranslatef(-0.5f,-0.5f,-0.5f);
      glColor3ubv(Joints);
			DrawUnitSphere(NumSegs);
      //DrawUnitCylinder(NumSegs);
			//drawer->draw(*par, GRAY);
  glPopMatrix();
}

void DrawBase(int NumSegs)
{
  glMatrixMode(GL_MODELVIEW);
  glPushMatrix();
    glScalef(0.2f,0.025f,0.2f);
    glTranslatef(-0.5f,0,-0.5f);
      glColor3ubv(Joints);
			DrawUnitSphere(NumSegs);
      //DrawUnitCylinder(NumSegs);
  glPopMatrix();
  glPushMatrix();
    glTranslatef(-0.05f,0,-0.05f);
    glScalef(0.1f,0.7f,0.1f);
      glColor3ubv(Arms);
      //DrawUnitCylinder(NumSegs);
			drawer->draw(*par, GRAY);
  glPopMatrix();
  glPushMatrix();
    glTranslatef(0,0.7f,0);
    DrawJoint(NumSegs);
  glPopMatrix();
}

void DrawArmSegment(int NumSegs)
{
  glMatrixMode(GL_MODELVIEW);
  glPushMatrix();
    glTranslatef(-0.05f,0,-0.05f);
    glScalef(0.1f,0.7f,0.1f);
      glColor3ubv(Arms);
      //DrawUnitCylinder(NumSegs);
			drawer->draw(*par, GRAY);
  glPopMatrix();
  glPushMatrix();
    glTranslatef(0,0.7f,0);
    DrawJoint(NumSegs);
  glPopMatrix();
}

void DrawWrist(int NumSegs)
{
  glMatrixMode(GL_MODELVIEW);
  glPushMatrix();
    glTranslatef(-0.04f,0,-0.04f);
    glScalef(0.08f,0.2f,0.08f);
      glColor3ubv(Fingers);
      DrawUnitCylinder(NumSegs);
  glPopMatrix();
  glPushMatrix();
    glTranslatef(0,0.2f,0);
    glScalef(0.12f,0.12f,0.12f);
    glTranslatef(-0.5f,-0.5f,-0.5f);
      glColor3ubv(FingerJoints);
      DrawUnitSphere(NumSegs);
  glPopMatrix();
}

void DrawFingerBase(int NumSegs)
{
  glMatrixMode(GL_MODELVIEW);
  glPushMatrix();
    glTranslatef(-0.025f,0,-0.025f);
    glScalef(0.05f,0.3f,0.05f);
      glColor3ubv(Fingers);
      DrawUnitCylinder(NumSegs);
  glPopMatrix();
  glPushMatrix();
    glTranslatef(0,0.3f,0);
    glScalef(0.08f,0.08f,0.08f);
    glTranslatef(-0.5f,-0.5f,-0.5f);
      glColor3ubv(FingerJoints);
      DrawUnitSphere(NumSegs);
  glPopMatrix();
}

void DrawFingerTip(int NumSegs)
{
  glMatrixMode(GL_MODELVIEW);
  glPushMatrix();
    glScalef(0.05f,0.25f,0.05f);
    glTranslatef(-0.5f,0,-0.5f);
      glColor3ubv(Fingers);
      DrawUnitCone(NumSegs);
  glPopMatrix();
}

void DrawRobotArm(int NumSegs)
{
  glMatrixMode(GL_MODELVIEW);
  glPushMatrix();

	/** Base of Arm **/
	glTranslatef(BaseTransX,0,BaseTransZ);
	glRotatef(BaseSpin,0.0,1.0,0.0);
	DrawBase(NumSegs);
	
	/** Arm Segment #1 **/
	glTranslatef(0,0.7f,0);
	glRotatef(ShoulderAng,0.0,0.0,1.0);
	DrawArmSegment(NumSegs);
		
	/** Arm Segment #2 **/
	glTranslatef(0,0.7f,0);
	glRotatef(ElbowAng,0.0,0.0,1.0);
	DrawArmSegment(NumSegs);
 
	/** Wrist **/
  glTranslatef(0,0.7f,0);
	glRotatef(WristAng,0.0,0.0,1.0);
	DrawWrist(NumSegs);

		glPushMatrix();
		
		/** Finger #1 **/
		glTranslatef(0,0.2f,0);
		glRotatef(FingerAng1,0.0,0.0,1.0);
		DrawFingerBase(NumSegs);

		/** Finger Tip **/
		glTranslatef(0,0.3f,0);
		glRotatef(FingerAng2,0.0,0.0,1.0);
		DrawFingerTip(NumSegs);

		glPopMatrix();	// Pop Finger 1


		glPushMatrix();

		/** Finger #2 **/
		glTranslatef(0,0.2f,0);
		glRotatef(-FingerAng1,0.0,0.0,1.0);
		DrawFingerBase(NumSegs);

		/** Finger Tip **/
		glTranslatef(0,0.3f,0);
		glRotatef(-FingerAng2,0.0,0.0,1.0);
		DrawFingerTip(NumSegs);


		glPopMatrix();	// Pop Finger 2

		glPushMatrix();

		/** Finger #3 **/
		glTranslatef(0,0.2f,0);
		glRotatef(FingerAng1,1.0,0.0,0.0);
		DrawFingerBase(NumSegs);

		/** Finger Tip **/
		glTranslatef(0,0.3f,0);
		glRotatef(FingerAng2,1.0,0.0,0.0);
		DrawFingerTip(NumSegs);


		glPopMatrix();	// Pop Finger 3

		glPushMatrix();

		/** Finger #4 **/
		glTranslatef(0,0.2f,0);
		glRotatef(-FingerAng1,1.0,0.0,0.0);
		DrawFingerBase(NumSegs);

		/** Finger Tip **/
		glTranslatef(0,0.3f,0);
		glRotatef(-FingerAng2,1.0,0.0,0.0);
		DrawFingerTip(NumSegs);


		glPopMatrix();	// Pop Finger 4


  glPopMatrix();				// Pop Base of Arm

}

void myDisplay()
{
  glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

  glMatrixMode(GL_MODELVIEW);
  glLoadIdentity();
  gluLookAt(eyex, eyey, eyez, atx, aty, atz, upx, upy, upz);

	lightPositions();

  DrawGroundPlane(NUMSEGS);
  DrawRobotArm(NUMSEGS);
	//drawer->draw(*par, 1., 0., 0.);
	drawer->draw(*esfera, 1., 0., 0.);
	drawer->draw(*pyr, 1., 0., 0.);
	drawer->draw(*tet, 1., 0., 0.);
	drawer->draw(*cyl, 1., 0., 0.);
	drawer->draw(*cone, 1., 0., 0.);

	if(locked)
		cameraMove(0.0, 0.0, 0.0, 0.0, 1.0, 0.0);
  glutSwapBuffers();
}

void myReshape(int w, int h)
{
  glViewport(0,0,w,h);

  glMatrixMode(GL_PROJECTION);
  glLoadIdentity();
  gluPerspective(30,(double)w/h,0.1,10);
}

void myIdle()
{
  glutPostRedisplay();
}

void KeyboardFunc(unsigned char Key, int x, int y)
{
	switch(Key) {
		case '1':
		case '2':
		case '3':
		case '4':
		case '5':	RobotControl = Key-'1'; break;
		case 'x':
			cyl->rotateX(rotateAngle, 0., 0., 0.);
			if(collision->collision(cyl, pyr)) {
				cyl->rotateX(-rotateAngle, 0., 0., 0.);
			}
			break;
		case 'y':
			cyl->rotateY(rotateAngle, 0., 0., 0.);
			if(collision->collision(cyl, pyr)) {
				cyl->rotateY(-rotateAngle, 0., 0., 0.);
			}
			break;
		case 'z':
			cyl->rotateZ(rotateAngle, 0., 0., 0.);
			if(collision->collision(cyl, pyr)) {
				cyl->rotateZ(-rotateAngle, 0., 0., 0.);
			}
			break;
		case 'c':
			lights[0] = !lights[0];
			if(lights[0]) glEnable(GL_LIGHT0);
			else glDisable(GL_LIGHT0);
			break;
		case 'v':
			lights[1] = !lights[1];
			if(lights[1]) glEnable(GL_LIGHT1);
			else glDisable(GL_LIGHT1);
			break;
		case 'b':
			lights[2] = !lights[2];
			if(lights[2]) glEnable(GL_LIGHT2);
			else glDisable(GL_LIGHT2);
			break;
		case 'n':
			lights[3] = !lights[3];
			if(lights[3]) glEnable(GL_LIGHT3);
			else glDisable(GL_LIGHT3);
			break;
		case 'm':
			lights[4] = !lights[4];
			if(lights[4]) glEnable(GL_LIGHT4);
			else glDisable(GL_LIGHT4);
			break;
		case 'p':
			lights[5] = !lights[5];
			if(lights[5]) glEnable(GL_LIGHT5);
			else glDisable(GL_LIGHT5);
			break;
		case 'l': locked = true; cameraMove(0.0, 0.0, 0.0, 0.0, 1.0, 0.0); break;
		case 'd': printf("distance point-segment = %lf\n", squareDistance(*p1, seg)); break;
		case 's': printf("distance point-point = %lf\n", squareDistance(*p1, *p2)); break;
			break;
		case 27: exit(0); break;
		default: return;
		
	}
	glutSwapBuffers();
}

static void specialKey( int k, int x, int y )
{
    switch ( k ) {
		case GLUT_KEY_UP:
				OldX=NewX; OldY=NewY;
  			NewX=x;    NewY=y;
				eyez -= 0.1;//(NewX-OldX) / (double)glutGet((GLenum)GLUT_WINDOW_WIDTH);
				//eyex += 0.1;
			break;
		case GLUT_KEY_DOWN:
				OldX=NewX; OldY=NewY;
			  NewX=x;    NewY=y;
				eyez += 0.1;//(NewX-OldX) / (double)glutGet((GLenum)GLUT_WINDOW_WIDTH);
				//eyex -= 0.1;
			break;
        case GLUT_KEY_LEFT:
					eyex -= 0.1;
					break;
        case GLUT_KEY_RIGHT:
						eyex += 0.1;
            break;

        default:
            return;
    }

    // Force a redraw of the screen in order to update the display
    glutPostRedisplay();
}

void MouseFunc(int button, int state, int x, int y)
{
  NewX=x; NewY=y;
  if (button==GLUT_LEFT_BUTTON)
    LeftButtonDown=!LeftButtonDown;
}

void MotionFunc(int x, int y)
{
  OldX=NewX; OldY=NewY;
  NewX=x;    NewY=y;
  double RelX = (NewX-OldX) / (double)glutGet((GLenum)GLUT_WINDOW_WIDTH);
  double RelY = (NewY-OldY) / (double)glutGet((GLenum)GLUT_WINDOW_HEIGHT);
  if (LeftButtonDown){
//cout << x << " " <<y<<" "<<RelX<<" "<<RelY<<" "<<GLUT_WINDOW_WIDTH<<endl;
    switch(RobotControl)
    {
      case 0: BaseTransX+=RelX; BaseTransZ+=RelY; break;
      case 1: BaseSpin+=RelX*180; break;
      case 2: 
				ShoulderAng+=RelY*-90;
				ElbowAng+=RelX*90;
				ShoulderAng = min(max(-90.0, ShoulderAng), 90.0);
				ElbowAng = min(max(-90.0, ElbowAng), 90.0);
				break;
      case 3:
				WristAng+=RelY*-180;
				WristTwistAng+=RelX*180;
				WristAng = min(max(-90.0, WristAng), 90.0);
				WristTwistAng = min(max(-90.0, WristTwistAng), 90.0);
				break;
      case 4:
				FingerAng1+=RelY*90;
				FingerAng2+=RelX*180;
				FingerAng1 = min(max(40.0, FingerAng1), 90.0);
				FingerAng2 = min(max(-90.0, FingerAng2), 40.0);
				break;
    };
  }
}

void init() {
	eyex = 5.0, eyey = 2.0, eyez = 4.0;
	atx = 0.0, aty = 0.5, atz = 0.0;
	upx = 0.0, upy = 1.0, upz = 0.0;
	par = new Parallelepiped(vec3(0.5,0.5,0.5), 1., 0.7, 0.7);
	esfera = new Sphere(vec3(5., 0., 0.), 0.1);
	pyr = new Pyramid(vec3(0.5,0.,0.), 0.2, 0.2);
	tet = new Tetrahedron(vec3(0.5,0.,0.5), 0.2);
	par2 = new Parallelepiped(vec3(0.,0.,0.), 0.2, 0.2, 0.2);
	pyr2 = new Pyramid(vec3(0.5,0.,0.), 0.2, 0.2);
	tet2 = new Tetrahedron(vec3(0.5,0.,0.5), 0.2);
	cyl = new Cylinder(vec3(0.,0.,0.5), 0.1, 0.2);
	cone = new Cone(vec3(0.,0.,1.), 0.1, 0.2);
	drawer = new Drawer();
	locked = false;

	p1 = new Point3D(2.5, 1., 0.);
	p2 = new Point3D(-0.5, 0.0, 0.0);
	p3 = new Point3D(2., 0.0, 0.0);
	seg = make_pair(*p2, *p3);

// Collision
	collision = new Collision();
	
	// Light
	GLfloat mat_specular[] = { 1.0, 1.0, 1.0, 1.0 };
	GLfloat mat_shininess[] = { 64.0 };
	GLfloat white_light[] = { 0.5, 0.5, 0.5, 1.0 };
	GLfloat lmodel_ambient[] = { 0.1, 0.1, 0.1, 1.0 };
	GLfloat spot_direction[] = { 0.0, -1.0, 0.0 };
	GLfloat spot_cut_off[] = { 15.0 };
	GLfloat spot_exponent[] = { 7.0 };
	glClearColor(0.0, 0.0, 0.0, 0.0);
	glShadeModel(GL_SMOOTH);
	
	// Materials
	glMaterialfv(GL_FRONT, GL_SPECULAR, mat_specular);
	glMaterialfv(GL_FRONT, GL_SHININESS, mat_shininess);
	
	// Lights
	// Light0
	glLightfv(GL_LIGHT0, GL_DIFFUSE, white_light);
	glLightfv(GL_LIGHT0, GL_SPECULAR, white_light);
	// Light1
	glLightfv(GL_LIGHT1, GL_DIFFUSE, white_light);
	glLightfv(GL_LIGHT1, GL_SPECULAR, white_light);
	// Light2
	glLightfv(GL_LIGHT2, GL_DIFFUSE, white_light);
	glLightfv(GL_LIGHT2, GL_SPECULAR, white_light);
	// Light3	
	glLightfv(GL_LIGHT3, GL_DIFFUSE, white_light);
	glLightfv(GL_LIGHT3, GL_SPECULAR, white_light);
	// Light4	
	glLightfv(GL_LIGHT4, GL_DIFFUSE, white_light);
	glLightfv(GL_LIGHT4, GL_SPECULAR, white_light);
	// Light5
	// SPOT LIGHT NAO FUNCIONA!!!!!!!!!!!
	glLightfv(GL_LIGHT5, GL_DIFFUSE, white_light);
	glLightfv(GL_LIGHT5, GL_SPECULAR, white_light);
	glLightfv(GL_LIGHT5, GL_SPOT_DIRECTION, spot_direction);
	glLightfv(GL_LIGHT5, GL_SPOT_CUTOFF, spot_cut_off);
	glLightfv(GL_LIGHT5, GL_SPOT_EXPONENT, spot_exponent);
	
	glLightModelfv(GL_LIGHT_MODEL_AMBIENT, lmodel_ambient);
	
	glEnable(GL_COLOR_MATERIAL);
	glEnable(GL_LIGHTING);	
	glEnable(GL_DEPTH_TEST);
	glEnable(GL_NORMALIZE);
	glEnable(GL_CULL_FACE);
	
	// Turn on the Lights
	glEnable(GL_LIGHT0);
	glEnable(GL_LIGHT1);
	glEnable(GL_LIGHT2);
	glEnable(GL_LIGHT3);
	glEnable(GL_LIGHT4);
	glEnable(GL_LIGHT5);
}


int main(int argc, char** argv)
{
  glutInit(&argc, argv);
	init();
  glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGB | GLUT_DEPTH);
  glutInitWindowSize(512,512);
  glutInitWindowPosition(180,100);
  glutCreateWindow("The Robot Arm");

  glutDisplayFunc(myDisplay);
  glutReshapeFunc(myReshape);
  glutIdleFunc(myIdle);

  glutKeyboardFunc(KeyboardFunc);
	glutSpecialFunc( specialKey );
  glutMouseFunc(MouseFunc);
  glutMotionFunc(MotionFunc);

  glutMainLoop();
  
  return EXIT_SUCCESS;
}
